#include "cGraph.h"

cGraph::cGraph(void)
{
	this->pather = new MicroPather(this);
}

cGraph::~cGraph(void)
{
}

bool cGraph::Load(char* filename)
{
	FILE *f;
	f=fopen(filename,"r");

	if(f == NULL)
	{
		printf("Cannot open %s for reading!\n", filename);
		return false;
	}

	int status = 0; // 0=start, 1=reading nodes, 2=reading connections

	char line[80];

	while(fgets(line, 80, f) != NULL)
	{
		switch(status)
		{
		case 0:
			{
			char tag[80];

			sscanf(line,"%s",tag);
			if(strcmp(tag,"[nodes]")==0)
				status = 1;
			else
				status = -1; // error
			}
			break;
		case 1:
			{
			char id[80];
			int x;
			int y;

			char tag[80];

			sscanf(line,"%s",tag);

			if(strcmp(tag,"[connections]")==0)
			{
				status = 2;
			}
			else
			{
			int n = sscanf(line,"%s%d%d",id,&x,&y);

				if(n==0)
				{
					status = -1; // error
				}
				else
				{
					cNode *node = new cNode(id,x,y);
					
					this->nodes.insert(NodePair(string(id),node));
				}
			}
			}
			break;
		case 2:
			{
			char from[80];
			char to[80];
			char costStr[80];
			int cost;

			int n = sscanf(line,"%s%s%s",from,to,costStr);
			
			cNode* fromNode = this->nodes.find(string(from))->second;
			cNode* toNode = this->nodes.find(string(to))->second;

			if(strcmp(costStr,"*")==0)
				cost = ComputeLinearCost(fromNode,toNode);
			else
				sscanf(costStr,"%d",&cost);

			cConnection *connectionFromTo = new cConnection(toNode,cost);
			cConnection *connectionToFrom = new cConnection(fromNode, cost);

			fromNode->neighbours->push_back(connectionFromTo);
			toNode->neighbours->push_back(connectionToFrom);
			}
			break;
		default:
			return false;
		}
	}

	fclose(f);

	return true;
}



void cGraph::Unload()
{
}

float cGraph::LeastCostEstimate( void* stateStart, void* stateEnd )
{
	cNode *start =(cNode*)stateStart;
	cNode *end = (cNode*)stateEnd;

	return ComputeLinearCost(start,end);
}

float cGraph::ComputeLinearCost(cNode* start, cNode* end)
{	
	return LinearCost(start->x,start->y,end->x,end->y);
}

float cGraph::LinearCost(int start_x, int start_y, int end_x, int end_y)
{
	int dx = start_x - end_x;
	int dy = start_y - end_y;

	return (float) sqrt( (double)(dx*dx) + (double)(dy*dy) );
}

void cGraph::AdjacentCost( void* state, std::vector< StateCost > *adjacent )
{
	cNode *node = (cNode*)state;

	ConnectionList *connections = node->neighbours; 

	ConnectionList::iterator it;

	for(it = connections->begin(); it!= connections->end(); ++it)
	{
		StateCost nodeCost = { (void*)((*it)->node), (float)(*it)->cost };
		adjacent->push_back( nodeCost );
	}
}

void cGraph::PrintStateInfo( void* state )
{
	printf("%s",(char*)state);
}

int cGraph::Find( cNode* startNode, cNode* endNode, vector< void* >* path, float* cost )
{
	return this->pather->Solve((void*)startNode, (void*)endNode, path, cost);
}

int cGraph::Find( int start_x, int start_y, int end_x, int end_y, vector< void* >* path, float* cost )
{
	cNode* startNode = NULL;
	cNode* endNode = NULL;

	bool startExists;
	bool endExists;

	float sCost;
	float eCost;
	//
	// check if a node with given x,y exists already, or get the closest one
	//
	startNode = FindClosestNode(start_x, start_y, &sCost, &startExists);
	endNode = FindClosestNode(end_x, end_y, &eCost, &endExists);

	int result = this->pather->Solve((void*)startNode, (void*)endNode, path, cost);

	if(result == MicroPather::SOLVED)
	{
		if(!startExists)
		{
			cNode* fakeStart = new cNode("fake_start",start_x,start_y);
			path->insert(path->begin(),fakeStart);
			*cost += sCost;
		}

		if(!endExists)
		{
			cNode* fakeEnd = new cNode("fake_end",end_x,end_y);
			path->push_back(fakeEnd);
			*cost += eCost;
		}
	}

	return result;
}

cNode* cGraph::FindClosestNode(int start_x, int start_y, float *cost, bool* nodeExists)
{
	//
	// This can be improved!
	//
	cNode* closestNode = NULL;
	float shortestPathLenght = 0.0f;

	*nodeExists = false;

	NodeMap::iterator it;

	for(it = nodes.begin(); it != nodes.end(); ++it)
	{
		cNode* node = (*it).second;

		// no candidates yet
		if(!closestNode)
		{
			shortestPathLenght = LinearCost(start_x,start_y,node->x,node->y);
			
			closestNode = node;
		}
		else
		{
			float pathLenght = LinearCost(start_x,start_y,node->x,node->y);

			if(pathLenght < shortestPathLenght)
			{
				shortestPathLenght = pathLenght;

				closestNode = node;
			}
		}

		// Given coords refer to an existing node!
		if((closestNode->x == start_x) && (closestNode->y == start_y))
		{
			*nodeExists = true;
			break;
		}
	}
	
	*cost = shortestPathLenght;

	return closestNode;
}